/*
 * CompoundPath.h
 *
 *  Created on: Mar 23, 2013
 *      Author: ohnozzy
 */

#ifndef COMPOUNDPATH_H_
#define COMPOUNDPATH_H_
#include "VehiclePath.h"
#include <vector>
namespace flyingcamera
{
    namespace base
    {
    class CompoundPath: public VehiclePath{
    protected:
    	std::vector<VehiclePath*> path;
    	std::vector<double> duration;
    public:

    	virtual void getLocation(double t, double& x, double& y, double& Yaw) const{
    	    if(t==1.0){
    	    	VehiclePath* p=path[duration.size()-1];
    	    	p->getLocation(1.0,x,y,Yaw);

    	    }else{
    	      for(unsigned i=0; i<duration.size();i++){
    	    	if(t<duration[i]){
    	    		double n=t/duration[i];
    	    		VehiclePath* p=path[i];
    	    		p->getLocation(n,x,y,Yaw);
    	    		return;
    	    	}else{
    	    		t-=duration[i];
    	    	}
    	    	//handling numeric error when sum duration[i]<1
    	    	if(i==(duration.size()-1)){
    	    		VehiclePath* p=path[i];
    	    		p->getLocation(1.0,x,y,Yaw);
    	    	}
    	      }
    	    }



        }
    	void addPath(VehiclePath* p, double d){
    		path.push_back(p);
    		duration.push_back(d);
    	}

    };
    }
}



#endif /* COMPOUNDPATH_H_ */
